#include "aEstrella.h"
#include <vector>
#include <stdlib.h>
#include <iostream>
using namespace std;

aEstrella::aEstrella(Mapa map) {

	escenario = map;
}

int aEstrella::encuentraRuta(int inicial, int encara_ini, int final, int encara_fin, vector<int> &path) {

	nodo node;
	int pos;
	bool fin = false;
	int padre, pos_padre;

	node.casilla = inicial;
	node.encaramiento = encara_ini;
	node.g = 0;
	node.h = costeH(inicial,node.encaramiento, final, encara_fin);
	node.f = node.g + node.h;
	node.posPadre = -1;
	cerrados.push_back(node);

	while (!fin) {
		aniadeSucesores(final, encara_fin);
		pos = encuentraCandidato();
		cerrados.push_back(abiertos[pos]);
		abiertos.erase(abiertos.begin() + pos);
		if (cerrados.back().casilla == final || abiertos.size()==0)
			fin = true;
	}
	cerrados.back().f += costeEncaramiento(cerrados.back().encaramiento, encara_fin);

	padre = cerrados.back().casilla;
	pos_padre = cerrados.size()-1;
	path.insert(path.begin(), padre);

	while((pos_padre = cerrados[pos_padre].posPadre) != -1)
		path.insert(path.begin(), cerrados[pos_padre].casilla);

	//********************Comprobaciones*********************************
	cout << "Cara inicial: " << encara_ini << " Cara final: " << encara_fin;
	cout << "\nRuta:  ";
	for (int i=0 ; i<path[0]; i++)
		cout << " " << path[i] << " ";
	cout << "\nCoste:  " << cerrados.back().f << " " << cerrados.back().g << " " << cerrados.back().h << "\n";
	//*********************************************************************

	return cerrados.back().f;
}

void aEstrella::aniadeSucesores(int final, int cara_final) {

	int pos;

	if (cerrados.back().casilla / 100 % 2) { //Casilla impar

		if( (pos = perteneceLista(cerrados.back().casilla - 1, abiertos)) != -1)	//Casilla superior
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla - 1, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla - 1, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla - 1, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla + 99, abiertos)) != -1)	//Casilla superior-derecha
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla + 99, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla + 99, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla + 99, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla - 101, abiertos)) != -1)	//Casilla superior-izquierda
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla - 101, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla - 101, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla - 101, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla +100, abiertos)) != -1)	//Casilla inferior-derecha
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla + 100, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla + 100, cerrados))
			aniadeNodo(cerrados.back().casilla + 100, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla - 100, abiertos)) != -1)	//Casilla inferior-izquierda
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla - 100, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla - 100, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla - 100, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla + 1, abiertos)) != -1)	//Casilla inferior
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla + 1, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla + 1, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla + 1, final, cara_final);
	}

	else { //Casilla par

		if( (pos = perteneceLista(cerrados.back().casilla - 1, abiertos)) != -1)	//Casilla superior
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla - 1, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla - 1, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla - 1, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla +100, abiertos)) != -1)	//Casilla superior-derecha
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla + 100, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla + 100, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla + 100, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla - 100, abiertos)) != -1)	//Casilla superior-izquierda
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla - 100, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla - 100, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla - 100, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla + 101, abiertos)) != -1)	//Casilla inferior-derecha
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla + 101, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla + 101, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla + 101, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla - 99, abiertos)) != -1)	//Casilla inferior-izquierda
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla - 99, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla - 99, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla - 99, final, cara_final);

		if( (pos = perteneceLista(cerrados.back().casilla + 1, abiertos)) != -1)	//Casilla inferior
			actualizaNodo(pos, final, cara_final);
		else if (transitable(cerrados.back().casilla + 1, cerrados.back().casilla)
				&& perteneceLista(cerrados.back().casilla + 1, cerrados) == -1)
			aniadeNodo(cerrados.back().casilla + 1, final, cara_final);
	}
}

void aEstrella::aniadeNodo(int numero, int final, int cara_final) {

	nodo node;

	node.casilla = numero;
	node.posPadre = cerrados.size() - 1;
	node.encaramiento = encaramientoHijo(numero);
	node.g = costeG(numero, node.encaramiento);
	node.h = costeH(numero, node.encaramiento, final, cara_final);
	node.f = node.g + node.h;
	abiertos.push_back(node);
}


int aEstrella::costeG(int n, int encaraN) {

	int G = cerrados.back().g;

	//**************Encaramiento***************************
	G += costeEncaramiento(cerrados.back().encaramiento, encaraN);

	//*********** Movimiento a la casilla **************************

	//Tipo de la casilla
	if (escenario.matriz[n % 100 - 1][n / 100 - 1].tipo == 0		//Abierto
			|| escenario.matriz[n % 100 - 1][n / 100 - 1].tipo == 1)		//Pavimentado
		G += 1;
	else if (escenario.matriz[n % 100 - 1][n / 100 - 1].tipo == 3)			//Pantanoso
		G += 2;
	else if (escenario.matriz[n % 100 - 1][n / 100 - 1].tipo == 2) {		//Agua
		if (escenario.matriz[n % 100 - 1][n / 100 - 1].nivel == 0)
			G += 1;
		else if (escenario.matriz[n % 100 - 1][n / 100 - 1].nivel == -1)
			G += 2;
		else
			G += 4;
	}
	//Objeto
	if (escenario.matriz[n % 100 - 1][n / 100 - 1].objeto == 0		//Escombros
			|| escenario.matriz[n % 100 - 1][n / 100 - 1].objeto == 1)		//Bosque ligero
		G += 1;
	else if (escenario.matriz[n % 100 - 1][n / 100 - 1].objeto == 2)		//Bosque denso
		G += 2;
	//Cambio de nivel
	G+= abs(escenario.matriz[n % 100 - 1][n / 100 - 1].nivel
			- escenario.matriz[cerrados.back().casilla % 100- 1][cerrados.back().casilla / 100 - 1].nivel);

	return G;
}


int aEstrella::costeH(int n, int caraN, int fin, int cara_final) {

	int H = 0;
	int diagonal, actual;

	if(n != fin){
		if (n / 100 == fin / 100) { //Se encuentran en la misma columna
			if (fin < n)
				H += costeEncaramiento(caraN, 1) + costeEncaramiento(1, cara_final) + n - fin;
			else
				H += costeEncaramiento(caraN, 4) + costeEncaramiento(4, cara_final) + fin - n;
		}

		else if (n < fin) { //La casilla destino se encuentra a la derecha
			if (casillaDiagonal(n, fin, 1) >= fin && casillaDiagonal(n, fin, 1) != 0) { //ZONA 1-2
				diagonal =casillaDiagonal(n, fin, 1);
				H += fin/100 - n/100;
				H += diagonal - fin;
					if (diagonal - fin != 0){
						H+=1;
						H+= min(costeEncaramiento(caraN, 1)+costeEncaramiento(2, cara_final),
								costeEncaramiento(caraN, 2)+costeEncaramiento(1, cara_final));
					}
					else
						H+=costeEncaramiento(caraN, 2)+costeEncaramiento(2, cara_final);
			}
			else if (casillaDiagonal(n, fin, 2) <= fin && casillaDiagonal(n, fin, 2) != 0) { //ZONA 3-4
				diagonal =casillaDiagonal(n, fin, 2);
				H += fin / 100 - n / 100;
				H += fin - diagonal;
					if (diagonal - fin != 0){
						H+=1;
						H+= min(costeEncaramiento(caraN, 3)+costeEncaramiento(4, cara_final),
								costeEncaramiento(caraN, 4)+costeEncaramiento(3, cara_final));
					}
					else
						H+= costeEncaramiento(caraN, 3)+costeEncaramiento(3, cara_final);
			}
			else {//ZONA 2-3
				H+= min(costeEncaramiento(caraN, 2)+costeEncaramiento(3, cara_final),
						costeEncaramiento(caraN, 3)+costeEncaramiento(3, cara_final));
				actual = n;

				do { //Subimos por la diagonal 1 (cara 2)
					if (actual / 100 % 2) //Casilla impar
						actual += 100 - 1;
					else
						//Casilla par
						actual += 100;
					H += 1;

				} while (casillaDiagonal(actual, fin, 2) != fin); //Comprobamos si la casilla buscada se encuentra
				//en la diagonal 2 (cara 3)
				H += 1;
				H += fin / 100 - actual / 100;
			}
		}

		else {

			if (casillaDiagonal(n, fin, 4) >= fin && casillaDiagonal(n, fin, 4) != 0) { //ZONA 6-1
				diagonal = casillaDiagonal(n, fin, 4);
				H += n / 100 - fin / 100;
				H += diagonal - fin;
					if (diagonal - fin != 0){
						H+=1;
						H+= min(costeEncaramiento(caraN, 1)+costeEncaramiento(6, cara_final),
								costeEncaramiento(caraN, 6)+costeEncaramiento(1, cara_final));
					}
					else
						H+= costeEncaramiento(caraN, 6)+costeEncaramiento(6, cara_final);
			}
			else if (casillaDiagonal(n, fin, 3) <= fin && casillaDiagonal(n, fin, 3) != 0) { //ZONA 4-5
				diagonal = casillaDiagonal(n, fin, 3);
				H += n / 100 - fin / 100;
				H += fin - diagonal;
					if (diagonal - fin != 0){
						H+=1;
						H+= min(costeEncaramiento(caraN, 4)+costeEncaramiento(5, cara_final),
								costeEncaramiento(caraN, 5)+costeEncaramiento(4, cara_final));
					}
					else
						H+= costeEncaramiento(caraN, 5)+costeEncaramiento(5, cara_final);

			}
			else { //ZONA 5-6
				H+= min(costeEncaramiento(caraN, 5)+costeEncaramiento(6, cara_final),
						costeEncaramiento(caraN, 6)+costeEncaramiento(5, cara_final));
				actual = n;
				do { //Subimos por la diagonal 4 (cara 6)
					if (actual / 100 % 2) //Casilla impar
						actual += -100 - 1;
					else	//Casilla par
						actual += -100;
					H += 1;

				}while (casillaDiagonal(actual, fin, 3) != fin); //Comprobamos si la casilla buscada se encuentra
																	//en la diagonal 3 (cara 5)
				H += 1;
				H += actual / 100 - fin / 100;
			}
		}
	}

	return H;

}

int aEstrella::encuentraCandidato() {

	int pos = 0;
	int dist = abiertos[0].f;

	for (unsigned int i = 0; i < abiertos.size(); i++) {
		if (abiertos[i].f < dist) {
			pos = i;
			dist = abiertos[i].f;
		}
	}

	return pos;
}

bool aEstrella::transitable(int siguiente, int actual) {


	if (siguiente/100 < 1 || siguiente/100 > escenario.ancho
			|| siguiente%100 < 1 || siguiente%100 > escenario.alto)
		return false;

	else if (escenario.matriz[siguiente % 100 - 1][siguiente / 100 - 1].ocupada
			|| (abs(escenario.matriz[siguiente % 100 - 1][siguiente / 100 - 1].nivel
					- escenario.matriz[actual % 100 - 1][actual / 100 - 1].nivel)) > 2)
		return false;

	return true;

}

int aEstrella::perteneceLista(int numero, vector <nodo> lista) {

	int pos = -1;

	for (unsigned int i = 0; i < lista.size(); i++){
		if (lista[i].casilla == numero)
			pos = i;
	}

	return pos;

}

int aEstrella::casillaDiagonal(int ini, int destino, int diagonal) {

	int cas_diagonal = 0;

	if (destino > 100) //Nos han introducido como parametro una casilla de la columna
		destino = destino / 100; //Encontramos la columna correspondiente

	if ( destino > 0 && destino <= escenario.ancho){
		if ((ini / 100 % 2) == 0) { //CASILLA PAR
			if ((diagonal == 1 && ini/100 < destino)|| (diagonal == 4 && ini/100 > destino)){
				if (ini%100 > abs(destino - ini / 100) / 2)
					cas_diagonal = (ini % 100) - (abs(destino - ini / 100) / 2)
						+ (destino * 100); //Diagonales hacia arriba
			}
			else if ((diagonal == 2 && ini/100 < destino) || (diagonal == 3 && ini/100 > destino)){
				if ((ini % 100) + (abs(destino - ini / 100) / 2 + 1) <= escenario.alto)
					cas_diagonal = (ini % 100) + ((abs(destino - ini / 100)+1) / 2)
						+ (destino * 100); //Diagonales hacia abajo
			}
		} else { //CASILLA IMPAR
			if ((diagonal == 1 && ini/100 < destino) || (diagonal == 4 && ini/100 > destino)){
				if ((ini % 100) > (abs(destino - ini / 100) / 2 + 1))
					cas_diagonal = (ini % 100) - ((abs(destino - ini / 100)+1) / 2)
						+ (destino * 100); //Diagonales hacia arriba
			}
			else if ((diagonal == 2 && ini/100 < destino)|| (diagonal == 3 && ini/100 > destino)){
				if ((ini % 100) + (abs(destino - ini / 100) / 2) <= escenario.alto)
					cas_diagonal = (ini % 100) + (abs(destino - ini / 100) / 2)
						+ (destino * 100); //Diagonales hacia abajo
			}
		}
	}

	return cas_diagonal;
}

int aEstrella::costeEncaramiento(int inicial, int final) {

	return min(abs(inicial - final), 6 - abs(inicial - final));

}

bool aEstrella::actualizaNodo(int pos, int final, int cara_final){

	int encaraN;
	bool actualizado = false;

	encaraN = encaramientoHijo(abiertos[pos].casilla);

	if(costeG(abiertos[pos].casilla, encaraN) + costeH(abiertos[pos].casilla, encaraN, final, cara_final)
			< abiertos[pos].f && transitable(abiertos[pos].casilla, cerrados.back().casilla)){

		abiertos[pos].g = costeG(abiertos[pos].casilla, encaraN);
		abiertos[pos].h = costeH(abiertos[pos].casilla, encaraN, final, cara_final);
		abiertos[pos].f = abiertos[pos].h + abiertos[pos].g;
		abiertos[pos].encaramiento = encaraN;
		abiertos[pos].posPadre= cerrados.size()-1;
		actualizado = true;
	}

	return actualizado;
}

int aEstrella::encaramientoHijo(int hijo){

	int encaramiento;

	//Casilla par
	if (!(cerrados.back().casilla / 100 % 2)) {
		if (cerrados.back().casilla - hijo == 1)
			encaramiento = 1;
		else if (cerrados.back().casilla - hijo == -100)
			encaramiento = 2;
		else if (cerrados.back().casilla - hijo == -101)
			encaramiento = 3;
		else if (cerrados.back().casilla - hijo == -1)
			encaramiento = 4;
		else if (cerrados.back().casilla - hijo == 99)
			encaramiento = 5;
		else if (cerrados.back().casilla - hijo == 100)
			encaramiento = 6;
	}

	//Casilla impar
	else {
		if (cerrados.back().casilla - hijo == 1)
			encaramiento = 1;
		else if (cerrados.back().casilla - hijo == -99)
			encaramiento = 2;
		else if (cerrados.back().casilla - hijo == -100)
			encaramiento = 3;
		else if (cerrados.back().casilla - hijo == -1)
			encaramiento = 4;
		else if (cerrados.back().casilla - hijo == 100)
			encaramiento = 5;
		else if (cerrados.back().casilla - hijo == 101)
			encaramiento = 6;
	}

	return encaramiento;
}


aEstrella::~aEstrella() {

}
